/*
 * @文件描述:一些简单的算法库
 * @版本:
 * @作者:周晨阳
 * @Date:2021-10-11 19:26:00
 */
#include "DSP.h"
//中位值平均滤波法（防脉冲干扰平均滤波法）
//方法解析：
//相当于中位值滤波+算术平均滤波，连续采样N个数据，去掉一个最大值和一个最小值，然后计算N-2个数据的算术平均值。
// N值的选取：3-14
//优点：融合了两种滤波法的优点
//对于偶然出现的脉冲性干扰，可消除由于脉冲干扰所引起的采样值偏差。
//缺点：
//测量速度较慢，和算法平均滤波一样，浪费RAM。

/**
 * @brief :中位值平均滤波法（防脉冲干扰平均滤波法）
 * @param {float} *ary 需要处理的数组
 * @param {u16} num 数据的个数
 * @param {float} MaxValue 数据可能会出现的最大值
 * @param {float} MinValue 数据可能会出现的最小值
 * @return {*}
 */
float filter(float *ary, u16 num, float MaxValue, float MinValue)
{
    u16 angleDataloopTemp2; //取平均的个数
    float finalData = 0;
    float maxValue = MinValue;
    float minValue = MaxValue;
    for (angleDataloopTemp2 = 0; angleDataloopTemp2 < num; angleDataloopTemp2++) //找出最大值和最小值
    {
        if (ary[angleDataloopTemp2] > maxValue) //去掉最大值最小值
        {
            maxValue = ary[angleDataloopTemp2];
        }
        if (ary[angleDataloopTemp2] < minValue) //去掉最大值最小值
        {
            minValue = ary[angleDataloopTemp2];
        }
        finalData += ary[angleDataloopTemp2];
    }
    finalData -= (maxValue + minValue); //去掉最大值最小值
    finalData /= (num - 2);             //取平均值
    return finalData;
}
//滑动平均滤波
//方法解析：
//把连续取N个采样值看成一个队列，队列的长度固定为N，每次采样到一个新数据放入队尾，并扔掉原来队首的一次数据（先进先出）。
//把队列中的N个数据进行算术平均运算，就可获得新的滤波结果。N值的选取：一般12.
//优点：
//对周期性干扰有良好的抑制作用，平滑度高，适应于高频振荡的系统
//缺点：
//灵敏度低，对偶然出现的脉冲性干扰的抑制作用较差。不易消除由于脉冲干扰所引起打的采样值偏差，不适用于脉冲干扰比较严重的场合
//浪费RAM
//************************************
// Method:    movingAverageFilter
// FullName:  movingAverageFilter
// Access:    public
// Returns:   float
// Qualifier: //滑动平均滤波,窗口值请改宏定义
// Parameter: float value：需要滤波的数值
//************************************
float movingAverageFilter(float value)
{
#define FILTER_N 180
    static float filter_buf[FILTER_N + 1];
    u8 i;
    float filter_sum = 0;
    filter_buf[FILTER_N] = value;
    for (i = 0; i < FILTER_N; i++)
    {
        filter_buf[i] = filter_buf[i + 1]; // 所有数据左移，低位仍掉
        filter_sum += filter_buf[i];
    }
    return (float)(filter_sum / FILTER_N);
}

/*
A、名称：消抖滤波法
B、方法：
设置一个滤波计数器，将每次采样值与当前有效值比较：
如果采样值=当前有效值，则计数器清零；
如果采样值<>当前有效值，则计数器+1，并判断计数器是否>=上限N（溢出）；
如果计数器溢出，则将本次值替换当前有效值，并清计数器。
C、优点：
对于变化缓慢的被测参数有较好的滤波效果；
可避免在临界值附近控制器的反复开/关跳动或显示器上数值抖动。
D、缺点：
对于快速变化的参数不宜；
如果在计数器溢出的那一次采样到的值恰好是干扰值,则会将干扰值当作有效值导入系统。
 */
//************************************
// Method:    shudderingFilter
// FullName:  shudderingFilter
// Access:    public
// Returns:   float
// Qualifier: 名称：消抖滤波法
// Parameter: float value：传进来的数值，如果不是float请强制转换成float
// Parameter: u8 filter_n：计数器的上限值，值越大滤波的结果越稳定，越小越不稳定，但是值太大数据会失真
//************************************
float shudderingFilter(float value, u8 filter_n)
{

    static u16 i = 0;                            //计数器
    static float lastValue = 0;                  //上一次的结果
    int newValue;                                //这一次的新的结果;
    newValue = value;                            //传值
    if (!(fabs(newValue - lastValue) < 0.0001f)) //如果上一次的结果不等于这一次的结果
    {
        i++;
        if (i > filter_n)
        {
            i = 0;
            lastValue = newValue;
        }
    }
    else //如果上一次的结果等于这一次的结果
    {
        i = 0;
    }
    return lastValue;
}

void init(Q *myqueue)
{
    myqueue->front = myqueue->rear = 0;
}
u8 FullQueue(Q *Q)
{
    if (Q->front == (Q->rear + 1) % Q_N) //判断循环链表是否满，留一个预留空间不用
        return 1;
    else
        return 0;
}
int isempty(Q *myqueue)
{
    if (myqueue->front == myqueue->rear)
    {
        return 1;
    }
    else
    {
        return 0;
    }
}
void enQueue(Q *myqueue, datatype num)
{

    if (FullQueue(myqueue))
        printf("enQueue error,full\n");
    else
    {
        myqueue->data[myqueue->rear] = num;
        myqueue->rear = (myqueue->rear + 1) % Q_N;
        return 1;
    }

}

datatype DeQueue(Q *myqueue)
{
    datatype data;
    if (myqueue->front == myqueue->rear)
    {
        printf("DeQueue error ,empty\n");
        return -1;
    }
    else
    {
        data = myqueue->data[myqueue->front];
        myqueue->front = (myqueue->front + 1) % Q_N;
        return data;
    }
}


